/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#include <iostream>
#include <cmath>
#include <exception>
#include <map>
#include <string>

using std::exception;
using std::map;
using std::string;

#include "incub.hh"
#include "ClientCallback.h"
#include "CorbaImplementations.h"
#include "CorbaHelper.h"
#include "boost/program_options.hpp"
#include <boost/thread/mutex.hpp>
#include "boost/format.hpp"
#include "Quaternion.h"
#include <GL/glut.h>

using boost::format;
namespace po = boost::program_options;
ClientCallback *callback;

class SensorCallback : public ClientCallback {
	private:
	string path;
	IGate_var gate;
	public:
	SensorCallback(string& path, IGate_var gate) {
		this->path = path;
		this->gate = gate;
	}
	void call() {
		std::cout << "callback called!" << std::endl;

		VSensorOut* sensor = gate->getVSensorOut(path.c_str());
	}
};

void processEntity(VEntityOut* entity, string& path, int n);

string formatNode(string path, int n, string name) {
	if(n==0) return(format("%s/%s") % path % name).str();
	else return (format("%s/%s[%d]") % path % name % (n+1)).str();
}

void processJoint(VJointOut* joint, string& _path, int n) {
	string path = formatNode(_path, n, "Joint");
	std::cout << path << std::endl;

	VMotorsOut* motors = joint->motors();
	for(int i = 0; i < motors->length(); i++) {
		string mpath = formatNode(path, i, "Motor");
		std::cout << mpath;

		VMotorOut* motor = (*motors)[i];
		double angle = motor->angle();
		double loStop = motor->loStop();
		double hiStop = motor->hiStop();

		VVelocityControlledMotorOut* velocityControlledMotor = VVelocityControlledMotorOut::_downcast(motor);
		if(velocityControlledMotor != NULL) {	
			std::cout << " (VelocityControlledMotor)";
		}

		VTorqueControlledMotorOut* torqueControlledMotor = VTorqueControlledMotorOut::_downcast(motor);
		if(torqueControlledMotor != NULL) {	
			std::cout << " (TorqueControlledMotor)";
		}
		std::cout << std::endl;
	}

	VEntityOut* entity = joint->entity();
	processEntity(entity, path, 0);
}

void processSensor(VSensorOut* sensor, string& _path, int n) {
	string path = formatNode(_path, n, "Sensor");
	std::cout << path;

	VCameraOut* camera = VCameraOut::_downcast(sensor);
	if(camera != NULL) {	
		std::cout << " (Camera)";
	}

	VAngularVelocimeterOut* angularVelocimeter = VAngularVelocimeterOut::_downcast(sensor);
	if(angularVelocimeter != NULL) {	
		std::cout << " (AngularVelocimeter)";
	}

	VLinearVelocimeterOut* linearVelocimeter = VLinearVelocimeterOut::_downcast(sensor);
	if(linearVelocimeter != NULL) {	
		std::cout << " (LinearVelocimeter)";
	}

	VPositionSensorOut* positionSensor = VPositionSensorOut::_downcast(sensor);
	if(positionSensor != NULL) {	
		std::cout << " (PositionSensor)";
	}
				
	VTactileMatrixOut* matrix = VTactileMatrixOut::_downcast(sensor);
	if(matrix != NULL) {
		std::cout << " (TactileMatrix)";		
	}
	std::cout << std::endl;
}

void processEntity(VEntityOut* entity, string& _path, int n) {
	string path = formatNode(_path, n, "Entity");
	std::cout << path << std::endl;
	
	VShapesOut* shapes = entity->shapes();
	for(int i = 0; i < shapes->length(); i++) {
		VShapeOut* shape = (*shapes)[i];

		string spath = formatNode(path, i, "Shape");
		std::cout << spath << std::endl;
	}
	
	VSensorsOut* sensors = entity->sensors();
	for(int i = 0; i < sensors->length(); i++) {
		VSensorOut* sensor = (*sensors)[i];
		processSensor(sensor, path, i);
	}
	
	VJointsOut* joints = entity->joints();
	for(int i = 0; i < joints->length(); i++) {
		VJointOut* joint = (*joints)[i];
		processJoint(joint, path, i);
	}
}

void processWorld(VWorldOut* world) {
	string path("/World");
	std::cout << path << std::endl;
	VEntitiesOut* entities = world->entities();
	for(int i = 0; i < entities->length(); i++) {
		VEntityOut* entity = (*entities)[i];
		processEntity(entity, path, i);		
	}
}

class ExitException : public std::exception {
};

class Renderer : public ClientCallback {
	protected:
	static map<int, Renderer*> renderers;
	public:
	virtual void call() {
	};
	static void _display(void) {
		int win = glutGetWindow();
		renderers[win]->display();
	}
	virtual void display() {
	};
	static void _keyb(unsigned char key, int x, int y) {
		int win = glutGetWindow();
		renderers[win]->keyb(key, x, y);
	}
	virtual void keyb(unsigned char key, int x, int y) {
	  std::cout << "Pressed key " << key << " on coordinates (" << x << "," << y << ")";
	  std::cout << std::endl;
	  if(key == 'q'){
	    std::cout << "Got q,so quitting " << std::endl;
	    throw ExitException();
	  }
	}
	static void _mouse(int button, int state, int x, int y) {
		int win = glutGetWindow();
		renderers[win]->mouse(button, state, x, y);
	}
	virtual void mouse(int button, int state, int x, int y) {
	}
	static void _motion (int x, int y) {
		int win = glutGetWindow();
		renderers[win]->motion(x, y);
	}
	virtual void motion (int x, int y) {
	}
};

map<int, Renderer*> Renderer::renderers;

class CameraRenderer : public Renderer {
	int win;
	string path;
	IGate_var gate;
	VCameraOut_var camera;
	boost::mutex mutex;
	public:
	CameraRenderer(IGate_var gate, string& path, int width, int height) {
	    this->path = path;
	    this->gate = gate;
	    this->camera = NULL;
		glutInitWindowSize(width,height);
		win = glutCreateWindow("Camera");
		renderers[win] = this;
		glutDisplayFunc(&_display);
	    glutKeyboardFunc(&_keyb);
	    glutIdleFunc(&idle);
		glClearColor (0.0, 0.0, 0.0, 0.0);
		glShadeModel(GL_FLAT);
	}
	void display() {
		if(camera == NULL) return;
		boost::mutex::scoped_lock scoped_lock(mutex);
		VImageOut* image = camera->image();
		glClear(GL_COLOR_BUFFER_BIT);
		glViewport(0, 0, camera->width(), camera->height());
		if(camera->depth() == 4) {
			glDrawPixels(camera->width(), camera->height(), GL_LUMINANCE, GL_FLOAT, image->_value().get_buffer()); 
		} else if(camera->depth() == 1) {
			glDrawPixels(camera->width(), camera->height(), GL_LUMINANCE, GL_UNSIGNED_BYTE, image->_value().get_buffer()); 
		} else {
			glDrawPixels(camera->width(), camera->height(), GL_RGB, GL_UNSIGNED_BYTE, image->_value().get_buffer()); 
		}
		glFlush();
		glutSwapBuffers();
	}
	static void idle() {
		usleep(1000);
	}
	void call() {
		boost::mutex::scoped_lock scoped_lock(mutex);
		camera = VCameraOut::_downcast(gate->getVSensorOut(path.c_str()));
		glutSetWindow(win);
		glutPostRedisplay();
	}
	~CameraRenderer() {
		glutDestroyWindow(win);
	}
};

class TactileMatrixRenderer : public Renderer {
	Quaternion start;			// orientation at the start of drag
	Quaternion now;			// quaternion "now" (while dragging)

	float downX, downY;	// where the mouse went down

	int win;
	string path;
	IGate_var gate;
	static int cur_x;
	static int cur_y;
	static int buttons[5];
	float scale;
	VTactileMatrixOut_var matrix;
	boost::mutex mutex;
	public:
	TactileMatrixRenderer(IGate_var gate, string& path, float scale) {
	    this->path = path;
	    this->gate = gate;
	    this->matrix = NULL;
	    this->scale = scale;
		glutInitWindowSize(640,480);
		win = glutCreateWindow("TactileMatrix");
		renderers[win] = this;
		glutDisplayFunc(&_display);
	    glutKeyboardFunc(&_keyb);
		glutMouseFunc(&_mouse);
		glutMotionFunc(&_motion);
		glutIdleFunc(&idle);

		GLfloat diffuse[] = {0.8, 0.8, 0.8, 1.0};
		GLfloat lgt1_diffuse[] = { 0.05f, 0.05f, 0.6f, 1.0f };
		GLfloat lgt2_diffuse[] = { 0.6f, 0.05f, 0.05f, 1.0f };
		GLfloat light_pos1[] =
		{ 5.0f, 5.0f, 0.0f, 1.0f };
		GLfloat light_pos2[] =
		{ -5.0f, 5.0f, 0.0f, 1.0f };

		glMaterialfv( GL_FRONT, GL_DIFFUSE, diffuse);
		glLightfv(GL_LIGHT1, GL_POSITION,light_pos1);
		glLightfv(GL_LIGHT1, GL_DIFFUSE, lgt1_diffuse);
		glLightfv(GL_LIGHT2, GL_POSITION,light_pos2);
		glLightfv(GL_LIGHT2, GL_DIFFUSE, lgt2_diffuse);
		glEnable(GL_LIGHT1);
		glEnable(GL_LIGHT2);
	}
	void display() {
		if(matrix == NULL) return;
		boost::mutex::scoped_lock scoped_lock(mutex);

		glEnable(GL_DEPTH_TEST);
		glShadeModel(GL_SMOOTH);
		glEnable(GL_LIGHTING);

		glEnable(GL_DEPTH_TEST);
		glEnable(GL_NORMALIZE);
		glEnable(GL_LIGHTING);
		glMatrixMode(GL_PROJECTION);
		glLoadIdentity();
		gluPerspective(65.0, (float) 640.0 / 480.0, 0.01, 1000);
		glTranslatef(0,0,-1);
	    glMatrixMode(GL_MODELVIEW);
	    glLoadIdentity();

		static GLdouble M[16];
		static double R[9];
		Quaternion rot = now * start;
		rot.getMatrix(R);
	    M[0]  = R[0]; M[1]  = R[3]; M[2]  = R[6];  M[3]  = 0;
	    M[4]  = R[1]; M[5]  = R[4]; M[6]  = R[7];  M[7]  = 0;
	    M[8]  = R[2]; M[9]  = R[5]; M[10] = R[8]; M[11]  = 0;
	    M[12] = 0; M[13] = 0; M[14] = 0;  M[15] = 1;
	    glMultMatrixd(M);

		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

		glScalef(1.0/scale, 1.0/scale, 1.0/scale);
		glBegin(GL_POINTS);
		VTactileSensorsOut *sensors = matrix->sensors();
		float maxActivation = 0;
		for(int i = 0; i < sensors->length(); i++) {
			VTactileSensorOut *sensor = (*sensors)[i];
			if(sensor->activation() > maxActivation) maxActivation = sensor->activation();
		}
		if(maxActivation == 0) maxActivation = 1;
		for(int i = 0; i < sensors->length(); i++) {
			VTactileSensorOut *sensor = (*sensors)[i];
			glPushMatrix();
			glTranslatef(sensor->x(), sensor->y(), sensor->z());
			glutSolidSphere(0.01*sensor->activation()/maxActivation+0.0025,32,32);
			glPopMatrix();
		}
		glEnd();
		glutSwapBuffers();
	}
	void mouse(int button, int state, int x, int y) {
		buttons[button] = state;

		start = now * start;
		now = Quaternion();		// identity
	
		downX = 1.0*(320-x)/320;
		downY = 1.0*(y-240)/240;
	}
	void motion (int x, int y) {
		float nowX = 1.0*(320-x)/320;
		float nowY = 1.0*(y-240)/240;

		float dx,dy,dz;
		float mx,my,mz;
		onUnitSphere(downX, downY, dx, dy, dz);
		onUnitSphere(nowX, nowY, mx, my, mz);
	
		// here we compute the quaternion between these two points
	    now.x = dy*mz - dz*my;
	    now.y = dz*mx - dx*mz;
	    now.z = dx*my - dy*mx;
	    now.w = dx*mx + dy*my + dz*mz;
	
		glutPostRedisplay();
	}
	static void idle() {
		usleep(1000);
	}	
	void call() {
		boost::mutex::scoped_lock scoped_lock(mutex);
		matrix = VTactileMatrixOut::_downcast(gate->getVSensorOut(path.c_str()));
		glutSetWindow(win);
		glutPostRedisplay();
	}
	~TactileMatrixRenderer() {
		glutDestroyWindow(win);
	}
	// a helper - figure out where in XYZ a mouse down goes
	// assumes sphere of unit radius
	static void onUnitSphere(const float mx, const float my, float& x, float& y, float& z) {
		x = mx;		// should divide radius
		y = my;
		float mag = x*x + y*y;
		if (mag > 1.0f) {
			float scale = 1.0f / ((float) sqrt(mag));
			x *= scale;
			y *= scale;
			z = 0;
		} else {
			z = (float) sqrt(1 - mag);
		}
	}
};

int TactileMatrixRenderer::buttons[5] = { GLUT_UP, GLUT_UP, GLUT_UP, GLUT_UP, GLUT_UP };
int TactileMatrixRenderer::cur_x = 0;
int TactileMatrixRenderer::cur_y = 0;
		
class RendererFactory {
	IGate_var gate;
	public:
	RendererFactory(IGate_var gate) {
		this->gate = gate;
	}
	Renderer *getRenderer(string& path) {
		VSensorOut_var sensor = gate->getVSensorOut(path.c_str());
		VCameraOut* camera = VCameraOut::_downcast(sensor);
		VTactileMatrixOut* matrix = VTactileMatrixOut::_downcast(sensor);
		if(camera != NULL) {
			return new CameraRenderer(gate, path, camera->width(), camera->height());
		} else if(matrix != NULL) {
			float minX = std::numeric_limits<double>::infinity(), maxX = -std::numeric_limits<double>::infinity();
			float minY = std::numeric_limits<double>::infinity(), maxY = -std::numeric_limits<double>::infinity();
			float minZ = std::numeric_limits<double>::infinity(), maxZ = -std::numeric_limits<double>::infinity();
			VTactileSensorsOut *sensors = matrix->sensors();
			for(int i = 0; i < sensors->length(); i++) {
				VTactileSensorOut *sensor = (*sensors)[i];
				if(sensor->x() > maxX) maxX = sensor->x();
				if(sensor->x() < minX) minX = sensor->x();
				if(sensor->y() > maxY) maxY = sensor->y();
				if(sensor->y() < minY) minY = sensor->y();
				if(sensor->z() > maxZ) maxZ = sensor->z();
				if(sensor->z() < minZ) minZ = sensor->z();
			}
			float scale = 0;
			if((maxX - minX) > scale) scale = maxX - minX;
			if((maxY - minY) > scale) scale = maxY - minY;
			if((maxZ - minZ) > scale) scale = maxZ - minZ;
			return new TactileMatrixRenderer(gate, path, scale);
		}
	}
};

int main(int argc, char** argv)
{
	CORBA::ORB_var orb;
	CORBA::BOA_var boa;
	IGate_var gate;

	po::variables_map vm;
	po::options_description desc("Allowed options");
	desc.add_options()
		("help", "produce help message")
		("listen,l", po::value<string>(), "listen to sensor")	    
		("set,s", po::value<string>(), "set motor parameters")	    
		("force,f", po::value<float>(), "motor maximum force (for VelocityControllableMotor)")	    
		("velocity,v", po::value<float>(), "motor velocity (for VelocityControllableMotor)")	
		("torque,t", po::value<float>(), "motor torque (for TorqueControllableMotor)")	
	;
			
	try {
		orb = CORBA::ORB_init(argc, argv, "omniORB4");

		CorbaHelper::registerFactories(orb);

		boa = orb->BOA_init(argc, argv, "omniORB4_BOA");
	
		CORBA::Object_var obj = CorbaHelper::getGateReference(orb);
	
		gate = IGate::_narrow(obj);

		po::store(po::parse_command_line(argc, argv, desc), vm);
		po::notify(vm);    
		
		if (vm.count("help")) {
		    std::cout << desc << "\n";
		    return 1;
		}
		
		obj = orb->resolve_initial_references("RootPOA");
		PortableServer::POA_var poa = PortableServer::POA::_narrow(obj);

		PortableServer::POAManager_var pman = poa->the_POAManager();
		pman->activate();
	
		if (vm.count("listen")) {
		    std::cout << "Listening to sensor " << vm["listen"].as<string>() << ".\n";
		    string path = vm["listen"].as<string>();

			glutInit(&argc, argv);
			glutInitDisplayMode(GLUT_RGBA | GLUT_DEPTH | GLUT_DOUBLE);
			
			RendererFactory *rendererFactory = new RendererFactory(gate);
			Renderer *renderer = rendererFactory->getRenderer(path);
			
			obj = orb->resolve_initial_references("RootPOA");
			PortableServer::POA_var poa = PortableServer::POA::_narrow(obj);
		
			PortableServer::ObjectId_var myCallbackId = poa->activate_object(renderer);
			gate->registerCallback(renderer->_this());
			
			try {
				glutMainLoop();	
			} catch(ExitException& ex) {
				gate->unregisterCallback(renderer->_this());
				poa->deactivate_object(myCallbackId);

				delete rendererFactory;
				delete renderer;
			}
			
		} else if(vm.count("set")) {
		    std::cout << "Setting parameters of motor " << vm["set"].as<string>() << ".\n";
		    string path = vm["set"].as<string>();

			VMotorOut_var motor = gate->getVMotorOut(path.c_str());
			VMotorIn_var motorIn = NULL;

			VVelocityControlledMotorOut* velocityControlledMotor = VVelocityControlledMotorOut::_downcast(motor);
			if(velocityControlledMotor != NULL) {	
				float force = vm["force"].as<float>();
				float velocity = vm["velocity"].as<float>();
				motorIn = new VVelocityControlledMotorIn_i(velocity, force);
			}
	
			VTorqueControlledMotorOut* torqueControlledMotor = VTorqueControlledMotorOut::_downcast(motor);
			if(torqueControlledMotor != NULL) {	
				float torque = vm["torque"].as<float>();
				motorIn = new VTorqueControlledMotorIn_i(torque);
			}
		
			gate->setVMotorIn(path.c_str(), motorIn);
		} else {
			VWorldOut_var world = gate->getVWorldOut();
			processWorld(world);
		}
		
		orb->destroy();

	} catch(CORBA::COMM_FAILURE& ex) {
		std::cerr << "Caught system exception COMM_FAILURE -- unable to contact the "
		 << "object." << std::endl;
		 exit(1);
	} catch(CORBA::MARSHAL& ex) {
		std::cerr << "Caught MARSHAL exception " << ex.NP_minorString() << std::endl;
		 exit(1);
	} catch(CORBA::BAD_PARAM& ex) {
		std::cerr << "Caught BAD_PARAM exception " << ex.NP_minorString() << std::endl;
		 exit(1);		
	} catch(CORBA::SystemException& ex) {
		std::cerr << "Caught a CORBA::SystemException."<< std::endl;
		 exit(1);
	} catch(CORBA::Exception& ex) {
		std::cerr << "Caught CORBA::Exception." << std::endl;
		 exit(1);
	} catch(omniORB::fatalException& fe) {
		std::cerr << "Caught omniORB::fatalException:" << std::endl;
		std::cerr << "file: " << fe.file() << std::endl;
		std::cerr << "line: " << fe.line() << std::endl;
		std::cerr << "mesg: " << fe.errmsg() << std::endl;
		 exit(1);
	} catch(...) {
		std::cerr << "Caught unknown exception." << std::endl;
		 exit(1);
	}
  
	return 0;
}
